#include "ethercatthread.h"
#include "messagelog.h"
#include <sys/mman.h>
#include <sched.h>


#include <fcntl.h>
 #include <sys/stat.h>
#define err_msg_n printf
#define O_RDWR		     02
static int latency_target_fd = -1;
static int32_t latency_target_value = 0;

inline int set_cpu(int i)
{
    cpu_set_t mask;
    CPU_ZERO(&mask);
    CPU_SET(i,&mask);

    printf("thread %u, i = %d\n", pthread_self(), i);
    if(-1 == pthread_setaffinity_np(pthread_self() ,sizeof(mask),&mask))
    {
        fprintf(stderr, "pthread_setaffinity_np erro\n");
        return -1;
    }
    return 0;
}




/* Latency trick
 * if the file /dev/cpu_dma_latency exists,
 * open it and write a zero into it. This will tell
 * the power management system not to transition to
 * a high cstate (in fact, the system acts like idle=poll)
 * When the fd to /dev/cpu_dma_latency is closed, the behavior
 * goes back to the system default.
 *
 * Documentation/power/pm_qos_interface.txt
 */
static void set_latency_target(void)
{
    struct stat s;
    int err;

//    if (laptop) {
//        warn("not setting cpu_dma_latency to save battery power\n");
//        return;
//    }

    int err_no = 0;
    err = stat("/dev/cpu_dma_latency", &s);
    if (err == -1) {
        err_msg_n(  "WARN: stat /dev/cpu_dma_latency failed");
        return;
    }

    err_no = 0;
    latency_target_fd = open("/dev/cpu_dma_latency", O_RDWR);
    if (latency_target_fd == -1) {
        err_msg_n(  "WARN: open /dev/cpu_dma_latency");
        return;
    }

    err_no = 0;
    err = write(latency_target_fd, &latency_target_value, 4);
    if (err < 1) {
        err_msg_n(  "# error setting cpu_dma_latency to %d!", latency_target_value);
        close(latency_target_fd);
        return;
    }
    printf("# /dev/cpu_dma_latency set to %dus\n", latency_target_value);
}

EthercatThread::EthercatThread(int sleeptime, QObject *parent) :
    QThread(parent)
{
    eCat=NULL;
    isRun = true;
    timeInval = sleeptime;

    //线程锁初始化
//    pthread_mutexattr_init(&attr);
//    pthread_mutexattr_setprotocol(&attr,PTHREAD_PRIO_INHERIT);
//    pthread_mutex_init(&ecatThreadMutex,&attr);
}

void EthercatThread::run()
{
    eCat=CoeEcatDevice::getInstance();
    eCat->cycle_ns=timeInval*1000;
    eCat->cycleStartTime();


    int max_cpus = sysconf(_SC_NPROCESSORS_ONLN);
    printf("Max CPUs = %d\n", max_cpus);


    /* lock all memory (prevent swapping) */
    if (mlockall(MCL_CURRENT|MCL_FUTURE) == -1) {
        perror("errrrrrrr mlockall\n");
    }

    set_latency_target();

    set_cpu(0);
#if 1

  struct sched_param param;
  param.__sched_priority = 99;
  sched_setscheduler(0,SCHED_FIFO,&param);
  //this->setStackSize(81920);
  int policy, priority;
  pthread_getschedparam(pthread_self(), &policy, (struct sched_param *)&priority);
  if(mlockall(MCL_CURRENT|MCL_FUTURE) == -1) {
      qDebug("mlockall failed");
  }

  static int reinit_count = 10;
  while(isRun) {
      if(eCat->state()==EC_ERR){
          if( reinit_count >0 ){
              --reinit_count;
              eCat->reInit();
          }else{
              qDebug()<<"ethercat 初始化失败!";
              eCat->isRequireReboot=true;
              usleep(1000000);//1s
          }
      }else{
          eCat->ecatUpdate( );
      }
  }
  eCat->~CoeEcatDevice();

#else

  /// clockid--值：CLOCK_MONOTONIC  , CLOCK_MONOTONIC  ， CLOCK_PROCESS_CPUTIME_ID  , CLOCK_THREAD_CPUTIME_ID
  /// evp--存放环境值的地址,结构成员说明了定时器到期的通知方式和处理方式等
  /// timerid--定时器标识符
 /* create a timer */
  struct sigevent evp;
  memset(&evp, 0, sizeof(struct sigevent));
  evp.sigev_notify = SIGEV_SIGNAL;//SIGEV_SIGNAL  ,  SIGEV_THREAD(通知的方式，派驻新线程 )
  evp.sigev_signo = SIGRTMIN;//SIGRTMIN实时信号(可靠信号)
  //  evp.sigev_value.sival_ptr = 0;  // 若是线程机制传递的参数
  //  evp.sigev_value.sival_int = 111;           // 信号机制传递的参数   //也是标识定时器的， 回调函数可以获得
  evp.sigev_notify_function =   timer_notify_fun;
  evp.sigev_notify_attributes = NULL; //  线程函数的属性  当 sigev_notify_attributes 非空，则制定该线程的属性
  timer_create(CLOCK_MONOTONIC, &evp, &timerid) ;

  ///  int timer_settime(timer_t timerid, int flags, const struct itimerspec *new_value,struct itimerspec *old_value);
  /// timerid--定时器标识
  /// flags--0表示相对时间，1表示绝对时间
  /// new_value--定时器的新初始值和间隔，如下面的it
  /// old_value--取值通常为0，即第四个参数常为NULL,若不为NULL，则返回定时器的前一个值
  ///第一次间隔it.it_value这么长,以后每次都是it.it_interval这么长,就是说it.it_value变0的时候会装载it.it_interval的值
  /* start a timer */
  struct timespec spec;
  struct itimerspec value;
  clock_gettime(CLOCK_MONOTONIC, &spec); /* CLOCK_MONOTONIC */
  value.it_value.tv_sec = spec.tv_sec ;
  value.it_value.tv_nsec = spec.tv_nsec + 1000000;//1ms
  value.it_interval.tv_sec = 0;    /* per second */
  value.it_interval.tv_nsec = 1000000;
  timer_settime(timerid, TIMER_ABSTIME, &value, NULL); /* TIMER_ABSTIME =0*/


  struct sched_param param;
  param.__sched_priority = 99;
  sched_setscheduler(0,SCHED_FIFO,&param);
  //this->setStackSize(81920);
  int policy, priority;
  // pthread_getschedparam(pthread_self(), &policy, (struct sched_param *)&priority);
  pthread_getschedparam(pthread_self(), &policy, (struct sched_param *)&priority);

#endif

    qDebug()<<"ethercat thread exit !";
    exec();
}
